/*
 * @brief: 反馈驱动
 * @date: 2015.09.06
 *
 * ChangeLog
 * Date             Author          Notes
 * 2015.12.16       Herbert         在fb.c的基础上修改成单系统的反馈
 */

#include "fb.h"
#include "dispatch.h"
#include "act_types.h"
#include "fb.h"
#include "cmd.h"
#include "feedback.h"
#include <assert.h>
#include "globe.h"
#include <string.h>
#include <stdio.h>

/*
 * @param state[out]: 错误反馈
 * @param type: 0-左机头，1-右机头
 */
int fb_get_error_state(MH_UDP *pudp, int *state, int type)
{
    short query = CMD_QUERY_FB_IN_CTEST;
    QUERY_FB_IN_CTEST_RESP resp;
    int ret = 0;
    int i = 0;
    ERROR_BITS error_bits;

    memset(&error_bits, 0, sizeof(error_bits));
    memset(&resp, 0, sizeof(resp));

    for (i = 0; i < 2; i++)
    {
        ret = cmd_query(pudp, (uchar*)&query, sizeof (query), (RESPONSE*)&resp, sizeof (resp));
        if (ret == 0)
        {
            error_bits.q1 = ((resp.cerror & (0x01 << 5)) != 0);
            error_bits.q2 = ((resp.cerror & (0x01 << 4)) != 0);
            error_bits.h1 = ((resp.cerror & (0x01 << 3)) != 0);
            error_bits.h2 = ((resp.cerror & (0x01 << 2)) != 0);
            error_bits.qxk = ((resp.cerror & (0x01 << 1)) != 0);
            error_bits.hxk = ((resp.cerror & (0x01 << 0)) != 0);

            memcpy(state, &error_bits, sizeof(int));
            break;
        }
    }

    if (i == 2)
    {
        return -3;
    }

    return ret;
}

/*
 * @param state[out]: 感应器反馈
 * @param type: 0-左机头，1-右机头
 */
int fb_get_sensor_state(MH_UDP *pudp, int *state, int type)
{
    short query = CMD_QUERY_FB_IN_CTEST;
    QUERY_FB_IN_CTEST_RESP resp;
    int ret = 0;
    int i = 0;
    SENSOR_BITS sensor_bits;

    memset(&sensor_bits, 0, sizeof(sensor_bits));

    memset(&resp, 0, sizeof(resp));

    for (i = 0; i < 2; i++)
    {
        ret = cmd_query(pudp, (uchar*)&query, sizeof (query), (RESPONSE*)&resp, sizeof (resp));

        if (ret == 0)
        {
            sensor_bits.q1 = ((resp.sensor & (0x01 << 5)) != 0);
            sensor_bits.q2 = ((resp.sensor & (0x01 << 4)) != 0);
            sensor_bits.h1 = ((resp.sensor & (0x01 << 3)) != 0);
            sensor_bits.h2 = ((resp.sensor & (0x01 << 2)) != 0);
            sensor_bits.qxk = ((resp.sensor & (0x01 << 1)) != 0);
            sensor_bits.hxk = ((resp.sensor & (0x01 << 0)) != 0);
            sensor_bits.fcam1 = sensor_bits.qxk;
            sensor_bits.bcam1 = sensor_bits.hxk;
            sensor_bits.fcam1_status = ((resp.sensor & (0x01 << 7)) != 0);
            sensor_bits.bcam1_status = ((resp.sensor & (0x01 << 6)) != 0);

            memcpy(state, &sensor_bits, sizeof(sensor_bits));
            break;
        }
    }

    if (i == 2)
    {
        return -3;
    }

    return ret;
}

int fb_get_crg_state(MH_UDP *pudp, CRGSTATE *crgstate, int type)
{
    assert(crgstate != NULL);

    short query = CMD_QUERY_FB_IN_CTEST;
    QUERY_FB_IN_CTEST_RESP resp;
    int ret = 0;
    memset(&resp, 0, sizeof(resp));

    ret = cmd_query(pudp, (uchar*)&query, sizeof (query), (RESPONSE*)&resp, sizeof (resp));
    if (ret == 0)
    {
        memcpy(crgstate->yc_state[0], resp.yc_state, 2);
        memcpy(crgstate->cam_state[0], resp.cam_state, 4);
        memcpy(crgstate->sel_state[0], resp.sel_state, 8);
        /* q1, q2, h1, h2, qxk, hxk, q3, q4, h3, h4 */
        memcpy(crgstate->motor_pos, resp.st_pos, 10 * sizeof (short));

        /* 三角位置 */
        crgstate->motor_pos[0][10] = resp.cam_pos[0];
        crgstate->motor_pos[0][11] = resp.cam_pos[1];
        crgstate->motor_pos[0][12] = resp.cam_pos[2];
        crgstate->motor_pos[0][13] = resp.cam_pos[3];

        int i = 0;
        for (i = 0; i < 14; i++)
        {
            printf("%d ", crgstate->motor_pos[0][i]);
        }
        printf("\n");
    }

    return ret;
}




